#include <iostream>
#include <cmath>
#include <tf2/LinearMath/Quaternion.h>

int main()
{
    double roll = 0.5;    // 弧度 - 绕X轴旋转
    double pitch = 2.0;   // 弧度 - 绕Y轴旋转  
    double yaw = 0.785;   // 弧度 - 绕Z轴旋转

    // 方法1：使用setEuler (参数顺序: yaw, pitch, roll)
    tf2::Quaternion q1;
    q1.setEuler(yaw, pitch, roll);

    // 方法2：使用setRPY (参数顺序: roll, pitch, yaw)  
    tf2::Quaternion q2;
    q2.setRPY(roll, pitch, yaw);

    std::cout << "setEuler result: " << q1.x() << ", " << q1.y() << ", " << q1.z() << ", " << q1.w() << std::endl;
    std::cout << "setRPY result: " << q2.x() << ", " << q2.y() << ", " << q2.z() << ", " << q2.w() << std::endl;

    // 正确的比较方法：考虑四元数的双覆盖特性和分量顺序
    tf2::Quaternion q1_normalized = q1.normalized();
    tf2::Quaternion q2_normalized = q2.normalized();
    
    // 方法1：直接比较（考虑符号差异）
    bool is_same_direct = (std::abs(q1_normalized.x() - q2_normalized.x()) < 1e-6 &&
                          std::abs(q1_normalized.y() - q2_normalized.y()) < 1e-6 &&
                          std::abs(q1_normalized.z() - q2_normalized.z()) < 1e-6 &&
                          std::abs(q1_normalized.w() - q2_normalized.w()) < 1e-6);

    // 方法2：考虑四元数双覆盖（q和-q表示相同旋转）
    bool is_same_negative = (std::abs(q1_normalized.x() + q2_normalized.x()) < 1e-6 &&
                            std::abs(q1_normalized.y() + q2_normalized.y()) < 1e-6 &&
                            std::abs(q1_normalized.z() + q2_normalized.z()) < 1e-6 &&
                            std::abs(q1_normalized.w() + q2_normalized.w()) < 1e-6);

    // 方法3：比较旋转矩阵是否相同
    tf2::Matrix3x3 m1(q1);
    tf2::Matrix3x3 m2(q2);
    bool is_same_matrix = true;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            if (std::abs(m1[i][j] - m2[i][j]) > 1e-6) {
                is_same_matrix = false;
                break;
            }
        }
        if (!is_same_matrix) break;
    }

    std::cout << "Direct comparison: " << (is_same_direct ? "Yes" : "No") << std::endl;
    std::cout << "Negative comparison: " << (is_same_negative ? "Yes" : "No") << std::endl;
    std::cout << "Matrix comparison: " << (is_same_matrix ? "Yes" : "No") << std::endl;
    std::cout << "Overall equivalent: " << ((is_same_direct || is_same_negative || is_same_matrix) ? "Yes" : "No") << std::endl;

    // 观察您的输出：setEuler的(x,y,z)对应setRPY的(z,x,y)
    std::cout << "\nNotice: setEuler(x,y,z) = setRPY(z,x,y)" << std::endl;
    std::cout << "setEuler(x,y,z): " << q1.x() << ", " << q1.y() << ", " << q1.z() << std::endl;
    std::cout << "setRPY(z,x,y): " << q2.z() << ", " << q2.x() << ", " << q2.y() << std::endl;

    return 0;
}